// Generated with RobotBuilder version 0.0.1
package org.usfirst.frc2876.Robot2013;
    
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
/**
 * The RobotMap is a mapping from the ports sensors and actuators are wired into
 * to a variable name. This provides flexibility changing wiring, makes checking
 * the wiring easier and significantly reduces the number of magic numbers
 * floating around.
 */
public class RobotMap {
    // The following variables are automatically assigned by
    // robotbuilder and will be updated the next time you export to
    // Java from robot builder. Do not put any code or make any change
    // in the following block or it will be lost on an update. To
    // prevent this subsystem from being automatically updated, delete
    // the following line.
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public static Jaguar SHOOTER_SHOOTINGJAGUAR;
    public static Jaguar SHOOTER_SHOOTINGANGLE_JAGUAR;
    public static Gyro DRIVETRAIN_GYRO;
    public static Encoder DRIVETRAIN_LEFTENCODER;
    public static Encoder DRIVETRAIN_RIGHTENCODER;
    public static Jaguar DRIVETRAIN_LEFTDRIVE_JAGUAR;
    public static Jaguar DRIVETRAIN_RIGHTDRIVE_JAGUAR;
    public static RobotDrive DRIVETRAIN_ROBOT_DRIVE_2;
    public static AnalogChannel SHOOTER_POT;
    public static Relay SHOOTER_FEEDER;
    public static DigitalInput HIGH_LM;
    public static DigitalInput LOW_LM;
    public static double MAXVOLT = 5;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public static void init() {
        // The following variables are automatically assigned by
        // robotbuilder and will be updated the next time you export to
        // Java from robot builder. Do not put any code or make any change
        // in the following block or it will be lost on an update. To
        // prevent this subsystem from being automatically updated, delete
        // the following line.
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        SHOOTER_SHOOTINGJAGUAR = new Jaguar(1, 10);
	LiveWindow.addActuator("Shooter", "ShootingJaguar", SHOOTER_SHOOTINGJAGUAR);
        
        SHOOTER_FEEDER = new Relay(1,8);
                
        LiveWindow.addActuator("Shooter", "ShooterFeeder", SHOOTER_FEEDER);
        
        SHOOTER_SHOOTINGANGLE_JAGUAR = new Jaguar(1, 2);
	LiveWindow.addActuator("AngleShooter", "ShootingAngle Jaguar", SHOOTER_SHOOTINGANGLE_JAGUAR);
        
        DRIVETRAIN_GYRO = new Gyro(1, 1);
	LiveWindow.addSensor("DriveTrain", "Gyro", DRIVETRAIN_GYRO);
        DRIVETRAIN_GYRO.setSensitivity(0.007);
        
        SHOOTER_POT = new AnalogChannel(1, 3); // Plug in the right ports
        LiveWindow.addActuator("AngleShooter", "Potentiometer", SHOOTER_POT);
        
        HIGH_LM = new DigitalInput(1, 6);      //Plug into right ports
        LiveWindow.addSensor("AngleShooter", "High Limit Switch", HIGH_LM);
        LOW_LM = new DigitalInput(1, 5);
        LiveWindow.addSensor("AngleShooter", "Low Limit Switch", LOW_LM);
        
        DRIVETRAIN_LEFTENCODER = new Encoder(1, 3, 1, 4, true, EncodingType.k4X);
	DRIVETRAIN_LEFTENCODER.setMinRate(10);
        DRIVETRAIN_LEFTENCODER.setMaxPeriod(10);
        DRIVETRAIN_LEFTENCODER.start();
        LiveWindow.addSensor("DriveTrain", "leftEncoder", DRIVETRAIN_LEFTENCODER);
        //DRIVETRAIN_LEFTENCODER.setDistancePerPulse(1.0);
        //DRIVETRAIN_LEFTENCODER.setPIDSourceParameter(PIDSourceParameter.kRate);
        
        DRIVETRAIN_RIGHTENCODER = new Encoder(1, 1, 1, 2, false, EncodingType.k4X);
	DRIVETRAIN_RIGHTENCODER.setMinRate(10);
        DRIVETRAIN_RIGHTENCODER.setMaxPeriod(10);
        DRIVETRAIN_RIGHTENCODER.start();
        LiveWindow.addSensor("DriveTrain", "rightEncoder", DRIVETRAIN_RIGHTENCODER);
        //DRIVETRAIN_RIGHTENCODER.setDistancePerPulse(1.0);
        //DRIVETRAIN_RIGHTENCODER.setPIDSourceParameter(PIDSourceParameter.kRate);
        
        DRIVETRAIN_LEFTDRIVE_JAGUAR = new Jaguar(1, 8);
	LiveWindow.addActuator("DriveTrain", "LeftDrive Jaguar", DRIVETRAIN_LEFTDRIVE_JAGUAR);
        
        DRIVETRAIN_RIGHTDRIVE_JAGUAR = new Jaguar(1, 9);
	LiveWindow.addActuator("DriveTrain", "RightDrive Jaguar", DRIVETRAIN_RIGHTDRIVE_JAGUAR);
        
        DRIVETRAIN_ROBOT_DRIVE_2 = new RobotDrive(DRIVETRAIN_LEFTDRIVE_JAGUAR, DRIVETRAIN_RIGHTDRIVE_JAGUAR);
	
        DRIVETRAIN_ROBOT_DRIVE_2.setSafetyEnabled(false);
        DRIVETRAIN_ROBOT_DRIVE_2.setExpiration(0.1);
        DRIVETRAIN_ROBOT_DRIVE_2.setSensitivity(0.5);
        DRIVETRAIN_ROBOT_DRIVE_2.setMaxOutput(1.0);
        
        DRIVETRAIN_ROBOT_DRIVE_2.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
        DRIVETRAIN_ROBOT_DRIVE_2.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        DRIVETRAIN_ROBOT_DRIVE_2.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
        DRIVETRAIN_ROBOT_DRIVE_2.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        
    }
    
     public static double roundtoTwo(double num) {
        return Math.floor(num * 100.0) / 100.0;
    }
}
